#include "matrix.h"

namespace krryn{
namespace math{
	matrix matrix::create_perspective_projection(float a_Near, float a_Far, float a_Width, float a_Height){
		matrix m;

		//m.cell[D0]		= (2 * a_Near) / a_Width;
		//m.cell[D1]		= (2 * a_Near) / a_Height;
		//m.cell[D2]		= (a_Far + a_Near) / (a_Far - a_Near);
		//m.cell[D2 + 1]	= -((2 * a_Far * a_Near) / (a_Far - a_Near));
		//m.cell[D3]		= 0;
		//m.cell[D3 - 1]	= 1;

		// RH DX
		m.cell[D0]		= (2 * a_Near) / a_Width;
		m.cell[D1]		= (2 * a_Near) / a_Height;
		m.cell[D2]		= (a_Far) / (a_Near - a_Far);
		m.cell[D2 + 1]	= -1;//
		m.cell[D3]		= 0;
		m.cell[D3 - 1]	= a_Near * a_Far / (a_Near - a_Far);

		
		// LH DX
		//m.cell[D0] = 2 * a_Near / a_Width;
		//m.cell[D1] = 2 * a_Near / a_Height;
		//m.cell[D2] = a_Far / (a_Far - a_Near);
		//m.cell[D2 + 1] = 1;
		//m.cell[D3] = 0;
		//m.cell[D3 - 1] = a_Near * a_Far / (a_Near - a_Far);

		return m;
	}

	quaternionf matrix::as_quaternion() const{
		//float *q = &jointQuats[i].q; 
		quaternionf result;
        //const float *m = jointMats[i].mat; 
		float *q = &result.m_X;
		const float *m = cell;
 
        if ( m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0f ) { 
 
            float t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
            float s = math::rsqrt( t ) * 0.5f; 
 
            q[3] = s * t; 
            q[2] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s; 
            q[1] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s; 
            q[0] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s; 
 
        } else if ( m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2] ) { 
 
            float t = + m[0 * 4 + 0] - m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f; 
            float s = math::rsqrt( t ) * 0.5f; 
 
            q[0] = s * t; 
            q[1] = ( m[0 * 4 + 1] + m[1 * 4 + 0] ) * s; 
			q[2] = ( m[2 * 4 + 0] + m[0 * 4 + 2] ) * s; 
			q[3] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s; 
		} else if ( m[1 * 4 + 1] > m[2 * 4 + 2] ) { 
			float t = - m[0 * 4 + 0] + m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f; 
			float s = math::rsqrt( t ) * 0.5f; 
			q[1] = s * t; 
			q[0] = ( m[0 * 4 + 1] + m[1 * 4 + 0] ) * s; 
			q[3] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s; 
			q[2] = ( m[1 * 4 + 2] + m[2 * 4 + 1] ) * s; 
		} else { 
			float t = - m[0 * 4 + 0] - m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
			float s = math::rsqrt( t ) * 0.5f; 
			q[2] = s * t; 
			q[3] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s; 
			q[0] = ( m[2 * 4 + 0] + m[0 * 4 + 2] ) * s; 
			q[1] = ( m[1 * 4 + 2] + m[2 * 4 + 1] ) * s; 
		} 
		//q[4] = m[0 * 4 + 3]; 
		//q[5] = m[1 * 4 + 3]; 
		//q[6] = m[2 * 4 + 3]; 
		//q[7] = 0.0f;

		return result;
	}

	std::ostream& operator<<(std::ostream& a_Stream, const matrix &a_Matrix){
		for(int i = 0; i < 4; i++){
			if(i == 0){
				a_Stream << '[';
			}else{
				a_Stream << ' ';
			}

			a_Stream << '[';

			for(int j = 0; j < 4; j++){
				a_Stream << a_Matrix[j + i * 4];

				if(j != 3){
					a_Stream << ", ";
				}
			}

			a_Stream << ']';

			if(i == 3){
				a_Stream << ']';
			}

			a_Stream << std::endl;
		}

		return a_Stream;
	}
}
}